﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_auxSlamStr.cs
 * \brief	This file contains the auxiliasy Sturctures definitions 
 *  
 * This file contains the ObjectIdentification function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodríguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2009-12-16
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ                       2009-12-16 \n
 *      First implementation of the M44_auxSlamStr class.
 *      
/* ######################################################################### */
using System;
/*This is for using lists*/
using System.Collections;
using System.Collections.Generic;
using System.Text;
/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Basis;
using Interfaces.Complex;
/*Using the C Sharp Matrix Library*/
using NMRS.CSML;
/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_auxSlamStr
     * 
     *  \brief  This is the class of the auxiliary structures for the slam data mapping.
     *
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_auxSlamStr
    {
        public struct cl_SlamResults
        {
            public double d_eSlam ;
            public double d_eSubSlam ;
            public double[] tf_PosSlam ;
            public double[] tf_PosSubSlam ;

            public cl_SlamResults( int n )  //n must be 2 always when calling the constructor
            {
                tf_PosSlam = new double[2];
                tf_PosSubSlam = new double[2];
                d_eSlam = -1;
                d_eSubSlam = -1;
                tf_PosSlam[0] = 0;
                tf_PosSlam[1] = 0;
                tf_PosSubSlam[0] = 0;
                tf_PosSubSlam[1] = 0;
            }
        }

        public struct cl_HRMResults
        {
            public cl_auxMapCell aux_MapCell_local_slam;
            public float[] RoundingError;

            public cl_HRMResults(int Nrows,int Ncols)
            {
                RoundingError = new float[3];
                aux_MapCell_local_slam = new cl_auxMapCell(Nrows,Ncols);
            }
        }

        public struct cl_SlamErrorsResults
        {
            public cl_SlamTest SlamTest;
            public cl_SlamTestErrors SlamTestErrors;
            public cl_SlamErrorsResults(cl_Map2D LowResMap)
            {
                SlamTest = new cl_SlamTest(2);
                SlamTestErrors = new cl_SlamTestErrors(LowResMap);
            }
        }

        public struct cl_auxMapCell
        {
            public float[,] Confidence;
            public float[,] Measured_Heights;
            public float[,] Measured_Gradient_X;
            public float[,] Measured_Gradient_Y;
            public float[,] Distance;

            public cl_auxMapCell(int Nrow,int Ncol)
            {
                Confidence = new float[Nrow, Ncol];
                Measured_Heights = new float[Nrow, Ncol];
                Measured_Gradient_X = new float[Nrow, Ncol];
                Measured_Gradient_Y = new float[Nrow, Ncol];
                Distance = new float[Nrow, Ncol];
                
            }
        }
        
        

        public struct cl_SlamTest
        {
            public struct cl_Test
            {
                public float Error_value;
                public int[] robotCellEstimate;
                public double[] robotPositionEstimate;
            }
            public cl_Test Gradient_total_error;
            public cl_Test Height_total_error;
            public cl_Test Global_total_error;

            public cl_SlamTest(int n)  //constructor
            {
                Gradient_total_error.Error_value = 0;
                Gradient_total_error.robotCellEstimate = new int[2];
                Gradient_total_error.robotPositionEstimate = new double[2];
                Height_total_error.Error_value = 0;
                Height_total_error.robotCellEstimate = new int[2];
                Height_total_error.robotPositionEstimate = new double[2];
                Global_total_error.Error_value = 0;
                Global_total_error.robotCellEstimate = new int[2];
                Global_total_error.robotPositionEstimate = new double[2];
            }
        }

        public struct cl_SumErrors
        {
            public float Err_sum_grad;
            public float Err_sum_height;
        }
        public struct cl_SubCellErrors
        {
            public float eTotalGrad;
            public float eTotalHeight;
            public float eTotalGlobal;
        }

        public struct cl_NewGrids
        {
            public double[,] eGrid;
            public double[, ,] PosGrid;
        }

        public struct cl_SlamTestErrors
        {
            public float[,] Gradient;
            public float[,] Height;
            public float[,] Height_max_abs;
            public float[,] Gradient_max_abs;
            public float[,] Gradient_total_error;
            public float[,] Height_total_error;
            public float[,] Global_total_error;
            public cl_SumErrors Sum_errors;

            public cl_SlamTestErrors(cl_Map2D LowResMapGlobal)
            {
                Gradient = new float[LowResMapGlobal.us_Nrows, LowResMapGlobal.us_Ncols];
                Height = new float[LowResMapGlobal.us_Nrows, LowResMapGlobal.us_Ncols];
                Height_max_abs = new float[LowResMapGlobal.us_Nrows, LowResMapGlobal.us_Ncols];
                Gradient_max_abs = new float[LowResMapGlobal.us_Nrows, LowResMapGlobal.us_Ncols];
                Gradient_total_error = new float[LowResMapGlobal.us_Nrows, LowResMapGlobal.us_Ncols];
                Height_total_error = new float[LowResMapGlobal.us_Nrows, LowResMapGlobal.us_Ncols];
                Global_total_error = new float[LowResMapGlobal.us_Nrows, LowResMapGlobal.us_Ncols];
                Sum_errors.Err_sum_grad = 0;
                Sum_errors.Err_sum_height = 0;
            }
        }

    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */